/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Watchdog;

/**
 *
 * @author Mark
 */
class TuneWheel
{
    public static void tuneWheel(Jaguar wheel, Joystick joystick, String wheelName, boolean chainsOut)
    {
        Watchdog.getInstance().feed();
       boolean selectionMade = false;

       System.out.println("\nTuning wheel: " + wheelName);

       while(joystick.getRawButton(Utils.FRONT_TRIG_BUTTON_NUM))
       {
        //   System.out.println("waiting.");
       }
       
       while(!selectionMade)
       {
         //  System.out.println("Twist: " + joystick.getTwist());
            if(joystick.getTwist() > .5)
            {
           //    System.out.println("Moving forward.");
                if(chainsOut)
                {
                    wheel.set(.2);
                }
                else
                {
                    wheel.set(-.2);
                }


            } else if(joystick.getTwist() < -.5)
            {
           //     System.out.println("Moving backwards.");
                if(chainsOut)
                {
                    wheel.set(-.1);
                }
                else
                {
                    wheel.set(.1);
                }
            } else {
                wheel.set(0);
            }

            if(joystick.getRawButton(Utils.FRONT_TRIG_BUTTON_NUM))
            {
           //     System.out.println("Stopping.");
                wheel.set(0);
                selectionMade = true;
            }
       }
    }
}
